#include "osc_api.h"
#include "ofc_ring_buf.h"
#include "bsp_axis_stm32f103.h"
#include "bsp_axis.h"
#include "bsp_axis_def.h"
#include "wit_c_sdk.h"
#include "REG.h"

static void Bsp_AxisWt61cWrite(uint8_t *p_ucData, uint32_t uiLen)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
    self->on_write(self, p_ucData, uiLen);
}

static void Bsp_AxisWt61cOnRecv(uint32_t uiReg, uint32_t uiRegNum)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
	int i;
    for(i = 0; i < uiRegNum; i++)
    {
        switch(uiReg)
        {
        // case AX:
        // case AY:
        case AZ:
            {
                if (self->acce_data != NULL)
                {
                    Bsp_TAxisAcceData data;
                    data.ax = sReg[AX] * 16000.0f / 32768.0f;
                    data.ay = sReg[AY] * 16000.0f / 32768.0f;
                    data.az = sReg[AZ] * 16000.0f / 32768.0f;

                    __disable_irq();
                    if (TOfRingBufWrite(&self->acce_ring_buf, &data, sizeof(data)) != sizeof(data))
                    {
                        TOfRingBufPop(&self->acce_ring_buf, sizeof(data));
                        TOfRingBufWrite(&self->acce_ring_buf, &data, sizeof(data));
                    }
                    __enable_irq();
                }
            }
            break;
        // case GX:
        // case GY:
        case GZ:
            {
                if (self->gyro_data != NULL)
                {
                    Bsp_TAxisGyroData data;
                    data.gx = sReg[GX] * 2000.0f / 32768.0f;
                    data.gy = sReg[GY] * 2000.0f / 32768.0f;
                    data.gz = sReg[GZ] * 2000.0f / 32768.0f;

                    __disable_irq();
                    if (TOfRingBufWrite(&self->gyro_ring_buf, &data, sizeof(data)) != sizeof(data))
                    {
                        TOfRingBufPop(&self->acce_ring_buf, sizeof(data));
                        TOfRingBufWrite(&self->gyro_ring_buf, &data, sizeof(data));
                    }
                    __enable_irq();
                }
            }
            break;
        // case HX:
        // case HY:
        case HZ:
            break;
        // case Roll:
        // case Pitch:
        case Yaw:
            {
                if (self->angle_data != NULL)
                {
                    Bsp_TAxisAngleData data;
                    data.roll = sReg[Roll] * 180.0f / 32768.0f;
                    data.pitch = sReg[Pitch] * 180.0f / 32768.0f;
                    data.yaw = sReg[Yaw] * 180.0f / 32768.0f;

                    if (TOfRingBufWrite(&self->angle_ring_buf, &data, sizeof(data)) != sizeof(data))
                    {
                        TOfRingBufPop(&self->angle_ring_buf, sizeof(data));
                        TOfRingBufWrite(&self->angle_ring_buf, &data, sizeof(data));
                    }
                }
            }
            break;
        default:
            break;
        }
		uiReg++;
    }
}

Bsp_THandle Bsp_AxisCreate(Int32 id)
{
    Bsp_THandle result = NULL;
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;

    if (id >= Bsp_kAxisIdBase && id < Bsp_kAxisIdMax)
    {
        self->acce_data = NULL;
        self->gyro_data = NULL;
        self->angle_data = NULL;
        self->acce_depth = 0;
        self->gyro_depth = 0;
        self->angle_depth = 0;
        self->is_open = False;

        self->axis_handle = Bsp_UartCreate(self->axis_id, self->send_buf, self->send_buf_size, self->recv_buf, self->recv_buf_size);
        if (self->axis_handle != NULL)
        {
	        WitInit(WIT_PROTOCOL_NORMAL, 0x50);

            WitDelayMsRegister((DelaymsCb)Osc_SleepMs); ///< 使用osc的sleep
            WitSerialWriteRegister(Bsp_AxisWt61cWrite);
            WitRegisterCallBack(Bsp_AxisWt61cOnRecv);

            result = (Bsp_THandle)self;
        }
    }

    return result;
}

void Bsp_AxisFree(Bsp_THandle handle)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
    if (handle == self)
    {
        Bsp_AxisClose(handle);
        
        WitDeInit();
        
        Bsp_UartFree(self->axis_handle);
    }
}

Int32 Bsp_AxisOpen(Bsp_THandle handle)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self)
    {
        if (!self->is_open)
        {
            if (self->acce_data == NULL && self->gyro_data == NULL && self->angle_data == NULL)
                result = Bsp_kErrorCodeProcess;
            else
            {
                if (self->acce_data != NULL)
                {
                    TOfRingBufCreate(&self->acce_ring_buf, self->acce_data, self->acce_depth * sizeof(*(self->acce_data)));
                }
                if (self->gyro_data != NULL)
                {
                    TOfRingBufCreate(&self->gyro_ring_buf, self->gyro_data, self->gyro_depth * sizeof(*(self->gyro_data)));
                }
                if (self->angle_data != NULL)
                {
                    TOfRingBufCreate(&self->angle_ring_buf, self->angle_data, self->angle_depth * sizeof(*(self->angle_data)));
                }

                if (Bsp_UartOpen(self->axis_handle, 115200, 0) >= 0)
                {
                    self->is_open = True;
                    result = Bsp_kErrorCodeSuccess;
                }
                else
                    result = Bsp_kErrorCodeFailed;
            }
        }
        else
            result = Bsp_kErrorCodeOpened;
    }

    return result;
}

Int32 Bsp_AxisClose(Bsp_THandle handle)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self)
    {
        if (self->is_open)
        {
            if (Bsp_UartClose(self->axis_handle) >= 0)
            {
                self->is_open = False;
                result = Bsp_kErrorCodeSuccess;
            }
            else
                result = Bsp_kErrorCodeFailed;
        }
        else
            result = Bsp_kErrorCodeClosed;
    }

    return result;
}

Int32 Bsp_AxisSetDataDepth(Bsp_THandle handle, Int8 data_type, void* data, Int32 depth)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self && (UInt8)data_type < Bsp_kAxisDataTypeMax && data != NULL && depth > 0)
    {
        if (!self->is_open)
        {
            if (data_type == Bsp_kAxisDataTypeAcce)
            {
                self->acce_data = data;
                self->acce_depth = depth;
                result = Bsp_kErrorCodeSuccess;
            }
            else if (data_type == Bsp_kAxisDataTypeGyro)
            {
                self->gyro_data = data;
                self->gyro_depth = depth;
                result = Bsp_kErrorCodeSuccess;
            }
            else if (data_type == Bsp_kAxisDataTypeAngle)
            {
                self->angle_data = data;
                self->angle_depth = depth;
                result = Bsp_kErrorCodeSuccess;
            }
            else
                result = Bsp_kErrorCodeNotSupport;
        }
        else
            result = Bsp_kErrorCodeOpened;
    }

    return result;
}

Int32 Bsp_AxisGetData(Bsp_THandle handle, Int8 data_type, void* data, Int32 count)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self && (UInt8)data_type < Bsp_kAxisDataTypeMax && data != NULL && count > 0)
    {
        if (self->is_open)
        {
            if (data_type == Bsp_kAxisDataTypeAcce)
            {
                if (self->acce_data != NULL)
                {
                    result = TOfRingBufRead(&self->acce_ring_buf, data, count * sizeof(*(self->acce_data)));
                    result = result / sizeof(*(self->acce_data));
                }
                else
                    result = Bsp_kErrorCodeProcess;
            }
            else if (data_type == Bsp_kAxisDataTypeGyro)
            {
                if (self->gyro_data != NULL)
                {
                    result = TOfRingBufRead(&self->gyro_ring_buf, data, count * sizeof(*(self->gyro_data)));
                    result = result / sizeof(*(self->gyro_data));
                }
                else
                    result = Bsp_kErrorCodeProcess;
            }
            else if (data_type == Bsp_kAxisDataTypeAngle)
            {
                if (self->angle_data != NULL)
                {
                    result = TOfRingBufRead(&self->angle_ring_buf, data, count * sizeof(*(self->angle_data)));
                    result = result / sizeof(*(self->angle_data));
                }
                else
                    result = Bsp_kErrorCodeProcess;
            }
            else
                result = Bsp_kErrorCodeNotSupport;
        }
        else
            result = Bsp_kErrorCodeClosed;
    }

    return result;
}
/// 轴水平安装
static void Bsp_AxisHorizontalInstall(void)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
	UInt8 send_data[3] = {0xff, 0xaa, 0x65};
    
    self->on_write(self, send_data, sizeof(send_data));
}

Int32 Bsp_AxisCalibrate(Bsp_THandle handle)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self)
    {
        if (self->is_open)
        {
            Bsp_AxisHorizontalInstall();
            result = Bsp_kErrorCodeSuccess;
        }
        else
            result = Bsp_kErrorCodeClosed;
    }

    return result;
}

Int32 Bsp_AxisRunOnce(Bsp_THandle handle)
{
    Bsp_TAxisAttr* self = &g_bsp_axis_attr;
    Int32 result = Bsp_kErrorCodeParam;

    if (handle == self)
    {
        if (self->is_open)
        {
            Int32 count;
            // 读取数据
            count = Bsp_UartRead(self->axis_handle, self->read_buf, self->read_buf_size);
            for (int i = 0; i < count; i++)
            {
                WitSerialDataIn(((UInt8*)self->read_buf)[i]);
            }

            result = Bsp_kErrorCodeSuccess;
        }
        else
            result = Bsp_kErrorCodeClosed;
    }

    return result;
}
